using System;
using System.Collections;
using UnityEngine;

public class PlanPathFindToPosition : Plan
{
	private AgentBehaviourScript agent;
	private Maze maze;
	private pathplanner pathPlanner = null;
	private Plan currentPlan = null;
	private Vector3 destination;
	private ArrayList obstacles = null;
	
	private int maxNodes = 2000;
	
	private bool pathComplete = false;
	
	public PlanPathFindToPosition(AgentBehaviourScript agent, Maze maze, Vector3 destination)
	{
		this.destination = destination;
		this.agent = agent;
		this.maze = maze;
	}

	public PlanPathFindToPosition(AgentBehaviourScript agent, Maze maze, Vector3 destination, ArrayList obstacles)
	{
		this.destination = destination;
		this.agent = agent;
		this.maze = maze;
		this.obstacles = obstacles;
	}
	
	public override bool execute()
	{
	

		
		if (pathPlanner	== null) /* Stage 1: Building path */ 
		{
			if (currentPlan	== null)
			{
				currentPlan = new PlanBuildAStarPath(agent.getPosition(), maze, destination, maxNodes,obstacles);
			}
		
			if (!currentPlan.execute())	
			{
				pathPlanner = ((PlanBuildAStarPath)currentPlan).getPathPlanner();	
				currentPlan = null;
			}
			
			return true; // Still building A*
		}
			
		//Execute the current plan if we have one
		if (currentPlan == null || !currentPlan.execute() && !pathComplete ) /* Stage 2: Following path */
		{
			
			//Create a new plan if we don't have a plan and there are more points in the path
			if (pathPlanner.has_next_point())
			{ 
				currentPlan = new PlanGoToPosition(agent, pathPlanner.next_path_vector(), maze.getCellSize()*3.0f);
			}else
			{
				// Path complete
				pathComplete = true;
				currentPlan = new PlanGoToPosition(agent, destination, maze.getCellSize()*0.7f);
				return true; // (disabled)Still have to go to destination from last point
			}
		}
		
		if (pathComplete) /* Stage 3: Going to final destination */
		{
			if (currentPlan == null || !currentPlan.execute())
			{
				currentPlan = null;	
				return false; // finished going to destination
			}
		}	
		//Plan isn't finished
		return true;
		
	}
	
	public override pathplanner getPathPlanner() {
		return pathPlanner;
	}

	public override string ToString()
	{
		string returnString = string.Format ("PathFind( ");
		if (pathComplete) 
			returnString += "Finishing ";
		if (currentPlan != null)
			returnString += currentPlan.ToString();
		returnString += " )";
		return returnString;
	}
	
	
}

